﻿/* ***************************************************************************
 *  This file is part of the C# port of the Invivo AI challenge simulation environment.
 *  Copyright 2010 Colin Green
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program .  If not, see <http://www.gnu.org/licenses/>.
 * 
 * 
 * This file incorporates open sourced work authored by the following people:
 *
 *   Copyright (c) 2009 Simon Funk
 */
using Box2DX.Common;
using Box2DX.Dynamics;

namespace FunkAIChallenge
{
    /// <summary>
    /// Interface for controlling a player. Methods are exposed for obtaining all required world state (ball velocity,
    /// position, arm state and a means of controlling the torque of each player arm joint)
    /// </summary>
    public class PlayerInterface
    {
        SimulationWorld _simWorld;
        Body _ball;
        Player _player;
        float[] _maxTorqueArr;
        float[] _appliedTorqueArr;

        #region Constructor

        /// <summary>
        /// Constructs the player interface with the provided ball and player objects and an array of maximum 
        /// joint torques to be enforced for the player arm.
        /// </summary>
        public PlayerInterface(SimulationWorld simWorld, Body ball, Player player, float[] maxTorqueArr)
        {
            _simWorld = simWorld;
            _ball = ball;
            _player = player;
            _maxTorqueArr = maxTorqueArr;
            _appliedTorqueArr = new float[3];
        }

        #endregion

        #region Public Properties [Arm State]

        /// <summary>
        /// Gets an array of joint angles. Note that zero degress corresponds to the first arm pointing up,
        /// the second pointing at 90 degress to the right and the hand poitning up (90 degress to the left 
        /// relative to the middle arm), this is consequence of the starting positions of the arms. These
        /// angles can of course be translated into whatever range is appropriate for a given usage.
        /// </summary>
        public float[] JointAngleArr
        {
            get { return _player.GetJointAngleArray(); }
        }

        /// <summary>
        /// Gets an array of joint angular velocities.
        /// </summary>
        public float[] JointVelocityArr
        {
            get { return _player.GetJointVelocityArray(); }
        }

        /// <summary>
        /// Gets an array of joint torques. These are the torques that have been applied via SetTorque() (truncated if over the max torque for a joint).
        /// Note that the actual torque applied to the Box2D joint will likely be different as we simulate joint friction by modifying the applied torque.
        /// </summary>
        public float[] JointTorqueArr
        {
            get { return _appliedTorqueArr; }
        }

        /// <summary>
        /// Gets the player hand center-of-mass position in world coords.
        /// </summary>
        public Vec2 HandPosition
        {
            get { return _player.HandBody.GetPosition(); }
        }

        /// <summary>
        /// Gets the player hand velocity.
        /// </summary>
        public Vec2 HandVelocity
        {
            get { return _player.HandBody.GetLinearVelocity() ; }
        }

        /// <summary>
        /// Gets the player hand angle in radians (relative to the world). Zero is up, CCW is positive.
        /// </summary>
        public float HandAngle
        {
            get { return _player.HandBody.GetAngle(); }
        }

        /// <summary>
        /// Gets the player hand angular velocity.
        /// </summary>
        public float HandAnglularVelocity
        {
            get { return _player.HandBody.GetAngularVelocity(); }
        }

        /// <summary>
        /// Gets the array of maximum joint torques being applied by the current interface.
        /// </summary>
        public float[] MaxTorqueArray
        {
            get { return _maxTorqueArr; }
        }

        #endregion

        #region Other State

        /// <summary>
        /// Gets the current simulation time.
        /// </summary>
        public float CurrentTime
        {
            get { return _simWorld.CurrentTime; }
        }

        /// <summary>
        /// Position of the player base (base block center of mass).
        /// </summary>
        public Vec2 BasePosition
        {
            get { return _player.BasePosition; }
        }

        /// <summary>
        /// Position of the player arm's first joint (attached to the top of the player base
        /// and thus slightly above BasePosition.
        /// </summary>
        public Vec2 ArmBasePosition
        {
            get { return _player.ArmBasePosition; }
        }

        /// <summary>
        /// Gets the ball's position.
        /// </summary>
        public Vec2 BallPosition
        {
            get { return _ball.GetPosition(); }
        }

        /// <summary>
        /// Gets the ball's velocity.
        /// </summary>
        public Vec2 BallVelocity
        {
            get { return _ball.GetLinearVelocity(); }
        }

        /// <summary>
        /// Gets a flag that indicates if there has been contact between hand and ball in the last timestep.
        /// </summary>
        public bool HandBallContact
        {
            get { return _simWorld.HandBallContact; }
        }


        #endregion

        #region Public [Control]

        /// <summary>
        /// Sets the torque of the specified joint. Note that torque limits defined at construction
        /// time are enforced here.
        /// </summary>
        public void SetJointTorque(int idx, float torque)
        {
            // Apply torque limits.
            float maxTorque = _maxTorqueArr[idx];
            if(torque > maxTorque) {
                torque = maxTorque;
            } else if(torque < -maxTorque) {
                torque = -maxTorque;
            }

            // Set joint torque.
            _player.SetJointTorque(idx, torque);

            // Store applied torque.
            _appliedTorqueArr[idx] = torque;
        }

        #endregion
    }
}
